#ifndef __APF_H
#define __APF_H
#include <stdlib.h>
#include <vector>
#include <cmath> 
#include <iostream>
#include <mutex>
using namespace std;

class APFPlanner
{
private:
    double sum(std::vector<double> p);
    vector<double> computeAngle();
    vector<double> computeAttraction(std::vector<double> att_angle);
    vector<double> computeRepulsion(std::vector<double> angle);
    /* data */

    double Attraction_K;
    double Repulsion_K;
    double a;
    double Obstacles_dis;

public:
    double start_point[3]{0.,0.,0.};
    double goal_point[2];
    vector<vector <double> > obstacles;
    int obs_num{0};
    std::mutex mtx;
    double dis_to_goal;
    double apf_angle;
    void init_apf(double ak,double rk,double aa,double dis)
    {
        Attraction_K = ak;
        Repulsion_K = rk;
        a =aa;
        Obstacles_dis = dis;
    }

    void set_start(double pos[3])
    {
        start_point[0] = pos[0];
        start_point[1] = pos[1];
        start_point[2] = pos[2];
    }

    void set_goal(double pos[3])
    {
        goal_point[0] = pos[0];
        goal_point[1] = pos[1];
        goal_point[2] = pos[2];
        dis_to_goal = sqrt(pow(goal_point[0]-start_point[0],2)+\
            pow(goal_point[1]-start_point[1],2));
    }

    void set_obstacle(vector<vector <double> > obs);

    double get_apf_angle();
};
#endif
